#include "../include/MapPoint.hpp"
namespace ORBSLAM
{
    long unsigned int MapPoint::sm_curid = 0;
    MapPoint::MapPoint(/* args */)
    {
    }

    MapPoint::~MapPoint()
    {
    }

    MapPoint::MapPoint(Mat &pos)
    {
        pos.copyTo(mm_world_pose);
        mn_obs = 0;
        mb_bad = false;
        mn_id = sm_curid++;

        mn_localmappoint_tmpid = -1;
        mn_lastseen_frameid = -1;

        mn_visible = 1;
        mn_found = 1;
    }

    MapPoint::MapPoint(Mat &pos, KeyFrame *p_KF, Map *p_map)
    {
    }

    void MapPoint::Set_ref_keframe(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2)
    {
        mvp_ref_keyframe.reserve(2);
        mvp_ref_keyframe.push_back(p_keyframe1);
        mvp_ref_keyframe.push_back(p_keyframe2);
        mn_first_keyframe_id = min(p_keyframe1->mn_keyframe_id, p_keyframe2->mn_keyframe_id);
    }

    void MapPoint::Set_map(Map *p_map)
    {
        mp_map = p_map;
    }

    void MapPoint::Add_key_frame(KeyFrame *KF)
    {
    }

    void MapPoint::Add_observation(KeyFrame *p_keyframe, int idx)
    {
        unique_lock<mutex> lock(mmutex_feature);
        int index;
        if (mmap_observation.count(p_keyframe))
        {
            index = mmap_observation[p_keyframe];
        }
        else
        {
            index = idx;
        }
        mmap_observation[p_keyframe] = index;
        mn_obs++;
    }

    map<KeyFrame *, int> MapPoint::Get_observation()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mmap_observation;
    }

    int MapPoint::Get_observation_num()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mn_obs;
    }

    Mat MapPoint::Get_descriptor()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mm_descrptor.clone();
    }

    bool MapPoint::Is_in_keyframe(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_feature);

        return (mmap_observation.count(p_keyframe));
    }

    bool MapPoint::Get_found_ratio()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return static_cast<float>(mn_found) / mn_visible;
    }

    void MapPoint::Set_bad_flag()
    {
        map<KeyFrame *, int> obs;
        {
            unique_lock<mutex> lock1(mmutex_feature);
            unique_lock<mutex> lock2(mmutex_pos);
            mb_bad = true;
            obs = mmap_observation;
            mmap_observation.clear();
        }
        for (auto observation : obs)
        {
            KeyFrame *p_keyframe = observation.first;
            int index = observation.second;
            p_keyframe->Erase_mappoint_match(index);
        }

        mp_map->Erase_mappoint(this);
    }

    Mat MapPoint::Get_normal()
    {
        unique_lock<mutex> lock(mmutex_pos);
        return mm_normal_vector.clone();
    }

    int MapPoint::Predict_scale(const float &current_dist, KeyFrame *p_keyframe)
    {
        float ratio;
        {
            unique_lock<mutex> lock(mmutex_pos);
            ratio = mf_max_distance / current_dist;
        }
        int n_scale = ceil(log(ratio) / p_keyframe->md_scale_factor);
        if (n_scale < 0)
        {
            n_scale = 0;
        }
        else if (n_scale >= p_keyframe->mi_nlevel)
        {
            n_scale = p_keyframe->mi_nlevel - 1;
        }
        return n_scale;
    }

    int MapPoint::Predict_scale(const float &current_dist, Frame *p_frame)
    {
        float ratio;
        {
            unique_lock<mutex> lock(mmutex_pos);
            ratio = mf_max_distance / current_dist;
        }
        int n_scale = ceil(log(ratio) / p_frame->md_scale_factor);
        if (n_scale < 0)
        {
            n_scale = 0;
        }
        else if (n_scale >= p_frame->mi_nlevel)
        {
            n_scale = p_frame->mi_nlevel - 1;
        }
        return n_scale;
    }

    void MapPoint::Replace(MapPoint *p_mpt)
    {
        if (p_mpt->mn_id == this->mn_id)
        {
            return;
        }
        int n_visible, n_found;

        map<KeyFrame *, int> obs;
        {
            unique_lock<mutex> lock1(mmutex_feature);
            unique_lock<mutex> lock2(mmutex_pos);
            obs = mmap_observation;
            mmap_observation.clear();
            mb_bad = true;
            n_visible = mn_visible;
            n_found = mn_found;
            mp_replaced_mpt = p_mpt;
        }

        for (auto observation : obs)
        {
            KeyFrame *p_keyframe = observation.first;
            int idx = observation.second;
            if (!p_mpt->Is_in_keyframe(p_keyframe))
            {
                p_keyframe->Replace_mappoint_match(idx, p_mpt);
                p_mpt->Add_observation(p_keyframe, idx);
                // p_keyframe->mn_mpts_num--;
            }
            else
            {
                p_keyframe->Erase_mappoint_match(idx);
            }
        }

        p_mpt->Increase_found(n_found);
        p_mpt->Increase_visible(n_visible);
        p_mpt->Compute_distinctive_descriptors();
        mp_map->Erase_mappoint(this);
    }

    void MapPoint::Increase_found(int n)
    {
        unique_lock<mutex> lock(mmutex_feature);
        mn_found += n;
    }

    void MapPoint::Increase_visible(int n)
    {
        unique_lock<mutex> lock(mmutex_feature);
        mn_visible += n;
    }

    void MapPoint::Erase_observation(KeyFrame *p_keyframe)
    {
        bool b_bad = false;
        {
            unique_lock<mutex> lock(mmutex_feature);
            if (mmap_observation.count(p_keyframe))
            {
                int idx = mmap_observation[p_keyframe];
                mn_obs--;
                mmap_observation.erase(p_keyframe);
                // if (mvp_ref_keyframe.size()==0)
                // {
                //     b_bad = true;
                // }
                if (mvp_ref_keyframe.size() == 1)
                {
                    if (p_keyframe == mvp_ref_keyframe[0])
                    {
                        b_bad = true;
                    }
                }
                else if (mvp_ref_keyframe.size() == 2)
                {
                    if (p_keyframe == mvp_ref_keyframe[0])
                    {
                        mn_first_keyframe_id = mvp_ref_keyframe[1]->mn_keyframe_id;
                        mvp_ref_keyframe.erase(mvp_ref_keyframe.begin());
                    }
                    else if (p_keyframe == mvp_ref_keyframe[1])
                    {
                        mn_first_keyframe_id = mvp_ref_keyframe[0]->mn_keyframe_id;
                        mvp_ref_keyframe.pop_back();
                    }
                }
                if (mn_obs <= 2)
                {
                    b_bad = true;
                }
            }
        }
        if (b_bad)
        {
            Set_bad_flag();
        }
    }

    int MapPoint::Get_index_in_keyframe(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_feature);
        if (mmap_observation.count(p_keyframe))
        {
            return mmap_observation[p_keyframe];
        }
        else
        {
            return -1;
        }
    }
    
    KeyFrame* MapPoint::Get_reference_keyframe()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mvp_ref_keyframe[0];
    }

    float MapPoint::Get_max_distance_invariance()
    {
        unique_lock<mutex> lock(mmutex_pos);
        return 1.2f * mf_max_distance;
    }

    float MapPoint::Get_min_distance_invariance()
    {
        unique_lock<mutex> lock(mmutex_pos);
        return 0.8f * mf_min_distance;
    }

    void MapPoint::Compute_distinctive_descriptors()
    {
        map<KeyFrame *, int> map_observation;
        {
            unique_lock<mutex> lock1(mmutex_feature);
            unique_lock<mutex> lock2(mmutex_pos);
            if (mb_bad)
            {
                return;
            }
            map_observation = mmap_observation;
        }
        if (map_observation.empty())
        {
            return;
        }
        vector<Mat> v_descriptor;
        v_descriptor.reserve(map_observation.size());
        for (map<KeyFrame *, int>::iterator mit = map_observation.begin(), mend = map_observation.end(); mit != mend; mit++)
        {
            KeyFrame *pKF = mit->first;
            if (!pKF->Is_bad())
            {
                v_descriptor.push_back(pKF->mm_descriptors.row(mit->second));
            }
        }
        if (v_descriptor.empty())
        {
            return;
        }
        const int n = v_descriptor.size();
        float distance[n][n];
        for (int i = 0; i < n; i++)
        {
            distance[i][i] = 0;
            for (int j = i + 1; j < n; j++)
            {
                int dist_ij = Matcher::Compute_descriptor_distance(v_descriptor[i], v_descriptor[j]);
                distance[i][j] = dist_ij;
                distance[j][i] = dist_ij;
            }
        }
        int best_median = INT_MAX;
        int bestidx = 0;
        for (int i = 0; i < n; i++)
        {
            vector<int> v_dist(distance[i], distance[i] + n);
            sort(v_dist.begin(), v_dist.end());
            int median = v_dist[(n - 1) / 2];
            if (median <= best_median)
            {
                best_median = median;
                bestidx = i;
            }
        }
        mm_descrptor = v_descriptor[bestidx].clone();
    }

    void MapPoint::Update_normal_and_depth()
    {
        map<KeyFrame *, int> map_observation;
        vector<KeyFrame *> vp_ref_keyframe;
        KeyFrame *p_ref_keyframe;
        Mat pose;
        {
            unique_lock<mutex> lock1(mmutex_feature);
            unique_lock<mutex> lock2(mmutex_pos);
            if (mb_bad)
            {
                return;
            }
            map_observation = mmap_observation;
            vp_ref_keyframe = mvp_ref_keyframe;
            p_ref_keyframe = mvp_ref_keyframe[0];
            pose = mm_world_pose.clone();
        }
        if (map_observation.empty())
        {
            return;
        }
        Mat normal = Mat::zeros(3, 1, CV_32F);
        int n = 0;
        for (map<KeyFrame *, int>::iterator mit = map_observation.begin(); mit != map_observation.end(); mit++)
        {
            KeyFrame *pKF = mit->first;
            int idx = mit->second;
            Mat normali = pose - pKF->Get_twc();
            normal = normal + normali / norm(normali);
            n++;
        }
        mm_normal_vector = normal / n;

        int m = vp_ref_keyframe.size();
        float max_distance;
        float min_distance;
        mf_max_distance = 0.0;
        mf_min_distance = 0.0;

        for (int i = 0; i < m; i++)
        {
            Mat PC = pose - vp_ref_keyframe[i]->Get_twc();
            const float dist = norm(PC);
            int index = map_observation[vp_ref_keyframe[i]];
            int level = vp_ref_keyframe[i]->mv_orb_unkeypoints[index].octave;
            const float level_scale_factor = vp_ref_keyframe[i]->mvd_scale_factor[level];
            const int nlevel = vp_ref_keyframe[i]->mi_nlevel;
            max_distance = dist * level_scale_factor;
            min_distance = max_distance / vp_ref_keyframe[i]->mvd_scale_factor[nlevel - 1];
            mf_max_distance += max_distance;
            mf_min_distance += min_distance;
        }
        mf_max_distance = mf_max_distance / m;
        mf_min_distance = mf_min_distance / m;

        // Mat PC = pose - p_ref_keyframe->Get_twc();
        // const float dist = norm(PC);
        // int index = map_observation[p_ref_keyframe];
        // int level = p_ref_keyframe->mv_orb_unkeypoints[index].octave;
        // const float level_scale_factor = p_ref_keyframe->mvd_scale_factor[level];
        // const int nlevel = p_ref_keyframe->mi_nlevel;
        // mf_max_distance = dist * level_scale_factor;
        // mf_min_distance = mf_max_distance / p_ref_keyframe->mvd_scale_factor[nlevel - 1];
    }

    bool MapPoint::Is_bad()
    {
        unique_lock<mutex> lock1(mmutex_feature, defer_lock);
        unique_lock<mutex> lock2(mmutex_pos, defer_lock);
        lock(lock1, lock2);
        return mb_bad;
    }

    void MapPoint::Set_world_pose(const Mat &pos)
    {
        unique_lock<mutex> lock(mmutex_pos);
        pos.copyTo(mm_world_pose);
    }

    Mat MapPoint::Get_world_pose()
    {
        unique_lock<mutex> lock(mmutex_pos);
        return mm_world_pose.clone();
    }

    Map *MapPoint::Get_map()
    {
        unique_lock<mutex> lock(mmutex_map);
        return mp_map;
    }

}